import rclpy 
from rclpy.node import Node 
from ai_msgs.msg import PerceptionTargets 

class UsbSubscriber(Node): 
    def __init__(self): 
        super().__init__('usb_subscriber') 
        self.subscription = self.create_subscription( 
            PerceptionTargets, 
            'hobot_dnn_detection',  
            self.listener_callback, 
            10) 
        self.subscription  # prevent unused variable warning 
        self.count = 0

    def listener_callback(self, msg):
        print("\n \033[31m---\033[0m This Frame: FPS = %d  \033[31m---\033[0m" % msg.fps)

        if len(msg.targets) == 0:
            print("No targets detected.")

        for num, target in enumerate(msg.targets):
            print(f"Target \033[0;32;40m{num}\033[0m: ")

            # 去除 type 中的回车符
            clean_type = target.rois[0].type.strip()
            print(f"Type: {clean_type}")

            if target.rois:
                #print(f"ROIs found. Number of ROIs: {len(target.rois)}")
                
                # 获取第一个 ROI
                roi = target.rois[0].rect

                # 获取框参数
                x_offset = roi.x_offset
                y_offset = roi.y_offset
                height = roi.height
                width = roi.width
                confidence =target.rois[0].confidence
                # 左上角、右上角、左下角、右下角的坐标
                top_left = (x_offset, y_offset)
                top_right = (x_offset + width, y_offset)
                bottom_left = (x_offset, y_offset + height)
                bottom_right = (x_offset + width, y_offset + height)

                # 中心坐标
                center = (int(x_offset + width / 2), int(y_offset + height / 2))

                print(f"Center: {center}")
                print(f"Height: {height}")
                print(f"Width: {width}")
                print(f"Confidence: {confidence}")
                

            else:
                print(f"No ROIs for target {num}")

# track  zhuitong qrcode

def main(args=None): 
    rclpy.init(args=args) 
    usb_subscriber = UsbSubscriber() 
    rclpy.spin(usb_subscriber) 
    usb_subscriber.destroy_node() 
    rclpy.shutdown() 

if __name__ == '__main__': 
    main() 
